#include "uwb_dt/uwb_dt_handler.h"
#include "std_msgs/String.h"

uwb_dt_handle::uwb_dt_handle(ros::NodeHandle& nh)
{
    // read config and register publishers
    std::vector<std::string> params;
    nh.getParam("topics", params);

    for (int i=0; i<params.size(); i++)
    {
        ros::Publisher pub = nh.advertise<std_msgs::String>(params.at(i), 100);
        pubs_.push_back(pub);
    }
    data_raw_sub_ = nh.subscribe("/nlink_linktrack_nodeframe0", 100, &uwb_dt_handle::DataRawCallback, this);
}

void uwb_dt_handle::DataBridge(const std::string& str_msg)
{
    if (str_msg.length() < 10)
        return;

    uint32_t data_length;
    uint32_t str_length;
    memcpy(&data_length, &str_msg[4], sizeof(uint32_t));
    str_length = data_length + 10;

    // verify frame head and tail
    if (str_msg[0] != FRAME_HEAD || str_msg[str_length-1] != FRAME_TAIL) 
        return;

    // verify topic index
    uint8_t index = static_cast<uint8_t>(str_msg[2]);
    uint8_t id = static_cast<uint8_t>(str_msg[3]);
    if (index > pubs_.size())    
        return;
 
    // extract data
    uint8_t sub_head = static_cast<uint8_t>(str_msg[1]);
    uint8_t sub_tail = static_cast<uint8_t>(str_msg[str_length-2]);
    std::string str_buffer;
    
    if (sub_head == FRAME_HEAD)
    {
        str_buffer = str_msg.substr(2, data_length + 6);
    }
    else if (sub_head == FRAME_RESUME)
    {
        if (msg_buffer_.find(id) == msg_buffer_.end())
            return;
        if (msg_buffer_[id].find(index) == msg_buffer_[id].end())
            return;

        str_buffer = msg_buffer_[id][index] + str_msg.substr(8, data_length);
        uint32_t* global_data_length = ((uint32_t*)&str_buffer[2]);
        *global_data_length = *global_data_length + data_length;
    }

    if (sub_tail == FRAME_PAUSE)
    {
        msg_buffer_[id][index] = str_buffer;
    }
    else if (sub_tail == FRAME_TAIL)
    {
        std_msgs::String pub_msg;
        pub_msg.data = str_buffer;
        pubs_.at(index-1).publish(pub_msg);

        msg_buffer_[id].erase(index);
    }
}

void uwb_dt_handle::DataRawCallback(const nlink_parser::LinktrackNodeframe0::ConstPtr& msg)
{
    std::string str_msg;

    for (auto node : msg->nodes)
    {
        str_msg.assign(node.data.begin(), node.data.end());
        DataBridge(str_msg);
    }
}